#include "QueueUtil.h"

// 初始化队列
void queue_init(Queue* q, int max_length) {
    q->front = q->rear = NULL;
    q->length = 0;
    q->max_length = max_length;
    pthread_mutex_init(&(q->mutex), NULL);
}

bool isEmpty(Queue* queue) {
    return (queue->length == 0);
}

int getLength(Queue* queue) {
    return queue->length;
}

// 入队 - 向队列尾部插入元素
bool enqueue(Queue* q, Quaternion data, Quaternion diffQ, int timeStamp) {

    Node* newNode = (Node*)malloc(sizeof(Node));
    newNode->timeStamp = timeStamp;
    newNode->q = data;
    newNode->diffQ = diffQ;
    newNode->next = NULL;

    if (q->length >= q->max_length) {
        return false;
    }

    pthread_mutex_lock(&(q->mutex));
    if (isEmpty(q)) {
        q->front = newNode;
        q->rear = newNode;
    } else {
        q->rear->next = newNode;
        q->rear = newNode;
    }
    q->length++;
    pthread_mutex_unlock(&q->mutex);

    return true;
}


// 出队 - 从队列的头部删除元素
bool dequeue(Queue* q, int *timeStamp, Quaternion* quaternion, Quaternion* diffQ) {
    pthread_mutex_lock(&(q->mutex));

    if (isEmpty(q)) {
        pthread_mutex_unlock(&(q->mutex));
        return false; // 队列为空
    }
    Node* temp = q->front;
    *timeStamp = temp->timeStamp;
    *diffQ = temp->diffQ;
    *quaternion = temp->q;

    q->front = q->front->next;
    free(temp);

    if (q->front == NULL) {
        q->rear = NULL;
    }
    q->length--;

    pthread_mutex_unlock(&(q->mutex)); // 解锁
    return true;
}


// 获取队列的头节点值
Node* getFront(Queue* queue) {
    if (queue->length == 0) {
        printf("队列为空，无法获取头节点值\n");
        exit(1);
    }
    return queue->front;
}

void clearQueue(Queue* q) {
    while (q->front != NULL) {
        Node* temp = q->front;
        q->front = q->front->next;
        free(temp);
    }
    q->rear = NULL;
    q->length = 0;
}

void destroyQueue(Queue* q) {
    while (q->front != NULL) {
        Node* temp = q->front;
        q->front = q->front->next;
        free(temp);
    }
    pthread_mutex_destroy(&(q->mutex));  // 销毁互斥锁
    free(q);
}

//复制队列第一个元素
Node* getFirstElement(const Queue* q) {
    if (q->front == NULL) {
        printf("Queue is empty!\n");
        return NULL;
    }

    Node* firstElement = (Node*)malloc(sizeof(Node));
    if (firstElement == NULL) {
        printf("Memory allocation failed!\n");
        return NULL;
    }

    firstElement->timeStamp = q->front->timeStamp;
    firstElement->q = q->front->q;
    firstElement->qInv = q->front->qInv;
    firstElement->diffQ = q->front->diffQ;
    firstElement->next = NULL;

    return firstElement;
}

// 获取队列的最后一个元素
Node* getLastElement(const Queue* q) {
    if (q->rear == NULL) {
        printf("Queue is empty!\n");
        return NULL;
    }

    Node* current = q->front;
    Node* lastElement = current;

    while (current != NULL) {
        lastElement = current;
        current = current->next;
    }

    Node* copyLastElement = (Node*)malloc(sizeof(Node));
    if (copyLastElement == NULL) {
        printf("Memory allocation failed!\n");
        return NULL;
    }

    copyLastElement->timeStamp = lastElement->timeStamp;
    copyLastElement->q = lastElement->q;
    copyLastElement->qInv = lastElement->qInv;
    copyLastElement->diffQ = lastElement->diffQ;
    copyLastElement->next = NULL;

    return copyLastElement;
}


/*************************************************************************************************************/
/*************************************        test cases      ************************************************/
/*************************************************************************************************************/

